import numpy as np
import matplotlib.pyplot as plt
import control

# 连续系统矩阵
A = np.array([
    [0, 1],
    [-10, -3]
])
B = np.array([
    [0],
    [10]
])
C = np.array([[1, 0]])
D = np.array([[0]])

# 连续状态空间系统
sys_c = control.ss(A, B, C, D)

# ====== 稳定性分析 ======
poles = control.poles(sys_c)
print("系统极点(特征值):", poles)

if np.all(np.real(poles) < 0):
    print("👉 系统稳定（所有极点实部都 < 0）")
elif np.any(np.real(poles) > 0):
    print("👉 系统不稳定（存在极点实部 > 0）")
else:
    print("👉 系统临界稳定（极点在虚轴上）")

# ====== 阶跃响应 ======
t = np.linspace(0, 5, 200)
t, y = control.step_response(sys_c, T=t)

fig, ax = plt.subplots()
ax.plot(t, y, label="Continuous", linewidth=3)

# ====== 离散化对比 ======
dt = 0.2
for method in ['zoh', 'tustin', 'euler', 'backward_diff']:
    sys_d = control.c2d(sys_c, dt, method=method)
    t_d, y_d = control.step_response(sys_d, T=np.arange(0, 5, dt))
    ax.step(t_d, y_d, label=method, where="post")

ax.set_xlim([0, 5])
ax.set_ylim([0, 2])
ax.legend(loc="best")
ax.set_xlabel("t [s]")
ax.set_ylabel("y(t)")
ax.set_title("Continuous vs Discrete Step Response")
plt.show()
